<!DOCTYPE html>
<html lang="zh">
  <head>
    <meta charset="utf-8">
    <meta name="viewport" content="width=device-width, initial-scale=1">
    <link rel="apple-touch-icon" href="apple-touch-icon.png">
    <link rel="shortcut icon" href="favicon.ico" />
    <link rel="manifest" href="a3.webmanifest">
    <title>A3 Robot UR5</title>
  </head>
  <body>
    <link rel="stylesheet" href="css/main.css"/>
    <script src="js/Theme.js"></script>
    <script src="js/ammo.js/build/ammo.wasm.js"></script>
    <div class="div-fullscreen"><div id="container"></div></div>
    <script type="module">
      import * as THREE from "./js/three.js/build/three.module.js";
      import URDFLoader from './jsm/URDFLoader/URDFLoader.js';
      import { ColladaLoader } from './jsm/loaders/ColladaLoader.js';

      import A3 from './js/a3.js';
      // import A3Class from "./js/A3Class.js";

      // let ta3 = new A3Class();
      // console.log(ta3);
      // await ta3.init();

      let mixer;

      let mixers = [];
      // const animationGroup = new THREE.AnimationObjectGroup();

      let ur5;
      let physicsUR5;

      const container = document.getElementById( 'container' );
      const a3 = await A3(container);
      
      const pos = new THREE.Vector3();
      const quat = new THREE.Quaternion();

      let clock = new THREE.Clock();

      const AxisX = new THREE.Vector3(1, 0, 0);
      const AxisY = new THREE.Vector3(0, 1, 0);
      const AxisZ = new THREE.Vector3(0, 0, 1);

      a3.addGround( 50, 0.2, 50, "textures/grid.png" );

      const Ammo = a3.AmmoLib;

      const joints = [];

      var defaultScale = 10;
      var showCollider = false;
      var showVisual = true;
      var disableGravity = false;
      var useCollider = true;

      // 测试墙
      const boxSize = 1;
      for(var x = 0; x < 10; x ++){
        for(var y = 0; y < 10; y++){
          const box = new THREE.Mesh( new THREE.BoxGeometry( boxSize * 2, boxSize, boxSize ), createMaterial() );
          pos.set( x * boxSize* 2 + boxSize - 5, y * boxSize + boxSize / 2, -6);
          quat.set(0, 0, 0, 1);
          a3.addMesh(box, .1, pos, quat);
        }
      }
      /////////////////////////////////////// 动画效果
      // POSITION
			const positionKF = new THREE.VectorKeyframeTrack( '.position', [ 0, 1, 2 ], [ 0, 0, 0, 0, 10, 0, 0, 0, 0 ] );

      // SCALE
      const scaleKF = new THREE.VectorKeyframeTrack( '.scale', [ 0, 1, 2 ], [ 1, 1, 1, 2, 2, 2, 1, 1, 1 ] );

      // set up rotation about x axis
			const xAxis = new THREE.Vector3( 1, 0, 0 );

      const qInitial = new THREE.Quaternion().setFromAxisAngle( xAxis, 0 );
      const qFinal = new THREE.Quaternion().setFromAxisAngle( xAxis, Math.PI );
      const quaternionKF = new THREE.QuaternionKeyframeTrack( '.quaternion', [ 0, 1, 2 ], [ qInitial.x, qInitial.y, qInitial.z, qInitial.w, qFinal.x, qFinal.y, qFinal.z, qFinal.w, qInitial.x, qInitial.y, qInitial.z, qInitial.w ] );
      
      // create an animation sequence with the tracks
			// If a negative time value is passed, the duration will be calculated from the times of the passed tracks array
			const clip = new THREE.AnimationClip( 'Action', 3, [ quaternionKF]);

      // 获取机械臂关节旋转轴 角度限制信息
      // 生成关键帧
      function createJointKeyframe( joint , isUpper = false ){

        isUpper = Math.random() > 0.5;
        const axis = joint.axis;
        const qInitial = new THREE.Quaternion().setFromAxisAngle( axis, 0 );
        const qFinal = new THREE.Quaternion().setFromAxisAngle( axis,  isUpper? joint.limit.upper * 0.25 : joint.limit.lower * 0.25);
        const kfs = [];
        const indexs = [];
        const count = 10;
        kfs.push(qInitial.x, qInitial.y, qInitial.z, qInitial.w);
        indexs.push(0);
        for( var i = 0; i < count; i++ ){
          const kf = new THREE.Quaternion().setFromAxisAngle( axis,  Math.random() * (joint.limit.upper - joint.limit.lower));
          kfs.push(kf.x, kf.y, kf.z, kf.w);
          indexs.push(i+1);
        }
        indexs.push(count+1);

        kfs.push(qInitial.x, qInitial.y, qInitial.z, qInitial.w);
        const quaternionKF = new THREE.QuaternionKeyframeTrack( '.quaternion', 
        indexs, 
        kfs);

        const clip = new THREE.AnimationClip( 'Action',count + 1 , [ quaternionKF ]);
        // console.log(qInitial, qFinal)
        return clip;

      }
      ///////////////////////////////////////////////
      
      //////////////////////////////////////////////////////////
      const manager = new THREE.LoadingManager();

      manager.onStart = function ( url, itemsLoaded, itemsTotal ) { 
        console.log( 'Started loading file: ' + url + '.\nLoaded ' + itemsLoaded + ' of ' + itemsTotal + ' files.' ); 
      }; 

      manager.onLoad = function ( ) { 
        console.log( 'Loading complete!');

        offLight(ur5);
        ur5.rotateX( - Math.PI / 2);
        ur5.setJointValue('shoulder_pan_joint',   10 / 180 * Math.PI);
        ur5.setJointValue('shoulder_lift_joint',  -20 / 180 * Math.PI);
        ur5.setJointValue('elbow_joint',          10 / 180 * Math.PI);
        ur5.setJointValue('wrist_1_joint',        60 / 180 * Math.PI);
        ur5.setJointValue('wrist_2_joint',        30 / 180 * Math.PI);
        ur5.setJointValue('wrist_3_joint',        45 / 180 * Math.PI);

        physicsUR5 = { links:{}, joints:{} };

        updateUR5();

        function clearUR5(){

          for( var key in physicsUR5.links ){ 
            a3.removeMesh( physicsUR5.links[key] ); 
          }

          for( var key in physicsUR5.joints ){ 
            physicsUR5.joints[key].dispose();
          }

          physicsUR5 = { links:{}, joints:{} };

          a3.removeMesh( ur5 );
        }

        function updateUR5(){

          clearUR5();


          for(var key in ur5.joints){
            ur5.setJointValue(key, 0)

            const mixer = new THREE.AnimationMixer( ur5.joints[key] );
            const clipAction = mixer.clipAction( createJointKeyframe( ur5.joints[key] ) );
            clipAction.play();
            mixers.push(mixer);

          }

          a3.addThreeObject( ur5 );

          // const va = new THREE.Vector3(1, 0, 0);
          // const vb = new THREE.Vector3(Math.PI / 2, 0, 0);
          // // va.multiplyVectors(vb);
          // console.log(va, vb);

          ur5.scale.set(defaultScale, defaultScale, defaultScale);
          ur5.position.set(0, 5, 0);

          console.log(ur5);

          var mass = 0
          const inertia = new Ammo.btVector3(0, 0, 0);
          const links = {};
          const joints = {};

          for( var key in ur5.links ){

            const link = ur5.links[key];
            
            if( link.children.length >= 2) {

              const visual = link.children[0];
              const collier = link.children[1];

              visual.visible = showVisual;
              collier.visible = showCollider;

              // 半透明碰撞模型

              const mesh = collier.children[0].children[0];
              // mesh.scale.set(1.1, 1.1, 1.1);
              mesh.material = createMaterial();
              mesh.material.transparent = true;
              mesh.material.opacity = 0.8;

              // 生成物理模型
              // 获取惯性参数
              const inertial = link.inertial;
              if( inertial ){
                mass = inertial.mass;
                inertia.setValue( 
                  inertial.inertia[0] * defaultScale, 
                  inertial.inertia[1] * defaultScale, 
                  inertial.inertia[2] * defaultScale 
                );
              }

              // if( key === 'base_link' ) mass = 0; 

              const shape = a3.createConvexHullPhysicsShape( mesh.geometry.attributes.position.array, defaultScale );

              // 常见复合模型

              const threeObject = useCollider ? mesh.clone() : visual.children[0].clone();
              threeObject.scale.set(defaultScale, defaultScale, defaultScale);

              // const compound = new a3.A3CompoundShape();
              // compound.addShape( shape, collier.position, collier.quaternion );
              
              a3.createRigidBody( 
                threeObject, shape, mass , 
                // null, null,
                a3.getWorldPosition( mesh ), a3.getWorldQuaternion( mesh ), 
                inertia
              );

              if( disableGravity ){ threeObject.userData.physicsBody.setGravity(0, 0, 0); }

              links[key] = threeObject;
            }
          }

          
          // 设置base坐标
          
          // 生成关节约束
          var pivotInA = new Ammo.btVector3( 0, 0, 0);
          var pivotInB = new Ammo.btVector3( 0, 0, 0);

          var axisInA = new Ammo.btVector3( 0, 1, 0);
          var axisInB = new Ammo.btVector3( 0, 1, 0);

          for( var key in ur5.joints ){
            continue;
            
            const joint = ur5.joints[key];
            if( joint._jointType === 'fixed' ) continue;

            const parent = joint.parent.name;
            const child = joint.children[0].name;
            
            const a3Joint = new a3.RevoluteJoint( links[parent], links[child], key );

            a3Joint.pivotInA.copy(joint.position);

            a3Joint.pivotInA.sub( ur5.colliders[parent].position ).multiplyScalar(defaultScale);
            a3Joint.pivotInB.sub( ur5.colliders[child].position ).multiplyScalar(defaultScale);
            
            a3Joint.axisInA.copy(joint.axis);
            a3Joint.axisInB.copy(joint.axis);

            a3Joint.axisInA.applyQuaternion(ur5.colliders[parent].quaternion.invert());

            a3Joint.init();

            // a3Joint.print();

            joints[key] = a3Joint;

          }
          /*
          // shoulder_pan_joint 
          // <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.089159"/>
          // <axis xyz="0 0 1"/>
          var jointName = "shoulder_pan_joint";
          pivotInA.setValue(0, 0, 0.089159 * defaultScale);
          pivotInB.setValue(0, 0, 0);
          axisInA.setValue(0, 0, 1);
          axisInB.setValue(0, 0, 1);
          joints[jointName].init( pivotInA, pivotInB, axisInA, axisInB );
          // joints[jointName].setJointVelocity(0.1);

          // shoulder_lift_joint
          // <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.13585 0.0"/>
          // <axis xyz="0 1 0"/>
          var jointName = "shoulder_lift_joint";
          pivotInA.setValue(0, 0.13585 * defaultScale, 0);
          pivotInB.setValue(0, 0, 0);
          axisInA.setValue(0, 1, 0);
          axisInB.setValue(0, 1, 0);
          joints[jointName].init( pivotInA, pivotInB, axisInA, axisInB );
          // joints[jointName].setJointVelocity(.1);

          // elbow_joint
          // <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.1197 0.425"/>
          // <axis xyz="0 1 0"/>
          var jointName = "elbow_joint";
          pivotInA.setValue(0, -0.1197 * defaultScale, 0.425 * defaultScale);
          pivotInB.setValue(0, 0, 0);
          axisInA.setValue(0, 1, 0);
          axisInB.setValue(0, 1, 0);
          joints[jointName].init( pivotInA, pivotInB, axisInA, axisInB );
          // joints[jointName].setJointVelocity(0.1);

          // wrist_1_joint
          // <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.0 0.39225"/>
          // <axis xyz="0 1 0"/>
          // <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
          var jointName = "wrist_1_joint";
          pivotInA.setValue(0, 0, 0.39225 * defaultScale);
          pivotInB.setValue(0, -0.093 * defaultScale, 0);
          axisInA.setValue(0, 1, 0);
          axisInB.setValue(0, 1, 0);
          joints[jointName].init( pivotInA, pivotInB, axisInA, axisInB );
          // joints[jointName].setJointVelocity(0.1);

          // wrist_2_joint
          // parent: <origin rpy="0.0 1.570796325 0.0" xyz="0.0 0.093 0.0"/>
          // <origin rpy="0.0 0.0 0.0" xyz="0.0 0.093 0.0"/>
          // <axis xyz="0 0 1"/>
          // <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>

          // const te = new THREE.Quaternion().setFromEuler(new THREE.Euler(0, Math.PI / 2, 0) );
          // const ta = new THREE.Vector3(0, 0, 1);
          
          // ta.applyQuaternion(te.invert());
          // console.log(ta, te);
  
          var jointName = "wrist_2_joint";
          pivotInA.setValue(0, 0, 0);
          pivotInB.setValue(0, 0, -0.09465 * defaultScale);
          axisInA.setValue(-1, 0, 0);
          axisInB.setValue(0, 0, 1);
          joints[jointName].init( pivotInA, pivotInB, axisInA, axisInB );
          // joints[jointName].setJointVelocity(0.1);

          // wrist_3_joint
          // <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
          // <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.09465"/>
          // <axis xyz="0 1 0"/>
          var jointName = "wrist_3_joint";
          pivotInA.setValue(0, 0, 0);
          pivotInB.setValue(0, 0, 0);
          axisInA.setValue(0, 1, 0);
          axisInB.setValue(0, 1, 0);
          joints[jointName].init( pivotInA, pivotInB, axisInA, axisInB );
          // joints[jointName].setJointVelocity(0.1);
          
          */
          // 保存物理模型
          physicsUR5.links = links;
          physicsUR5.joints = joints;
        }

        a3.addInputNumber(null, '缩放', 
          (val)=>{ 
            defaultScale = val;
            updateUR5();
          }, 
          [1, 100], defaultScale 
        );

        a3.addCheckbox(null, '显示碰撞模型', 
          (val)=>{ 
            showCollider = val;
            updateUR5();
          }, showCollider 
        );

        a3.addCheckbox(null, '显示图形模型', 
          (val)=>{ 
            showVisual = val;
            updateUR5();
          }, showVisual 
        );

        a3.addCheckbox(null, '使用碰撞模型', 
          (val)=>{ 
            useCollider = val;
            updateUR5();
          }, useCollider 
        );

        a3.addCheckbox(null, '取消重力', 
          (val)=>{ 
            disableGravity = val;
            updateUR5();
          }, disableGravity
        );

        // 添加关节速度控制
        for(var key in ur5.joints){
          
          const min = ur5.joints[key].limit.lower * 180 / Math.PI;
          const max = ur5.joints[key].limit.upper * 180 / Math.PI;

          const ctrl = a3.addInputNumber(null, key, (val)=>{
              updateJointValue( ctrl.id, val );              
            },
            [ min, max ], 0
          );

          ctrl['id'] = key;
        }
        
        // ur5.setJointValue('wrist_2_joint', Math.PI / 4);
        // ur5.visible = false;       
      }; 

      function updateJointValue( joint, value ){
        const rad = value * Math.PI / 180;

        ur5.setJointValue( joint, rad );
      }

      manager.onProgress = function ( url, itemsLoaded, itemsTotal ) { 
        console.log( 'Loading file: ' + url + '.\nLoaded ' + itemsLoaded + ' of ' + itemsTotal + ' files.' ); 
      }; 

      manager.onError = function ( url ) { 
        console.log( 'There was an error loading ' + url ); 
      }; 

      const loader = new URDFLoader( manager );
      loader.parseCollision = true;
      loader.parseInertial = true;
      const ur5UrdfFile = 'files/ur_description/urdf/ur5_robot.urdf';

      loader.packages = {
        ur_description:'files/ur_description',
      };

      loader.load(ur5UrdfFile, robot => {
        ur5 = robot;
      });

      animate();
      
      function offLight(node){
        if(node.children){
          for(let i=0, il=node.children.length; i < il; i++){
            const child = node.children[i];
            if(child.children){
              offLight(child);
            }
          }
        }
        if(node.type === 'DirectionalLight'){
          node.visible = false;
        }
      }
      
      function createMaterial( color ) {

        return new THREE.MeshPhongMaterial( { 
          color: color?color:createRandomColor(), 
          transparent: false, 
          opacity: 0.5,
          wireframe:false,
        } );

      }

      function createRandomColor() {

        return Math.floor( Math.random() * ( 1 << 24 ) );

      }
    

      function animate() {

        requestAnimationFrame( animate );

        render();

      }

      
      var deltaCount = 0;

      function render() {

        const delta = clock.getDelta();
        deltaCount += delta;

				mixers.forEach(mixer => {

          mixer.update( delta );

        });


        debug();
        a3.update();

      }


      let t = 0;
      let dir = -1;
      function debug(){

        // if(deltaCount < 2) return;
        // deltaCount = 0;
        if(physicsUR5){

          // for( var key in physicsUR5.joints ){
          //   const joint = physicsUR5.joints[key];

          //   // physicsUR5.joints[key].update();
            
          //   const q1 = joint.parent.quaternion.clone().invert();
          //   const q2 = joint.child.quaternion;
          //   q1.multiply(q2);
          //   const rotaion = new THREE.Euler().setFromQuaternion(q1);

          //   console.log(key,  
          //     (rotaion.x * 180 / Math.PI ).toFixed(2), 
          //     (rotaion.y  * 180 / Math.PI).toFixed(2), 
          //     (rotaion.z  * 180 / Math.PI).toFixed(2));

          // }
          // console.log("------------------")
          for(var key in physicsUR5.links){
            // console.log(key, 
            //   physicsUR5.links[key].userData.physicsBody.getLinearVelocity().z())
            a3.updateBody(ur5.links[key].children[0], physicsUR5.links[key].userData.physicsBody);
            // console.log(key, physicsUR5.links[key].rotation.x);
          }
        }
      }

      
    </script>
  </body>
</html>